
#include "Sim.h"

using namespace std;

/*----------------------------------------------------------------------------*/
Sim::Sim(int pNumMoons, int pNumDebris) {

	isThin = false;
	thinRdy = false;

	//get the number of moons and add 1 for the planet
	numBodies = pNumMoons +1;

	numDebris = pNumDebris;

	srand( (unsigned int)time(NULL) );
	G = (6+(2/3.0))*0.0001;

	userInit();

	timeNow = timePrev = clock();
	timePass = 0;
	timeDT = 0.05f;
}

/*----------------------------------------------------------------------------*/
Sim::Sim() {

	//ONLY CALL TO INIT A THIN SIM
	isThin = true;
	thinRdy = false;

	numBodies = 0;
	numDebris = 0;

	timeNow = timePrev = clock();
	timePass = 0;
	timeDT = 0.05f;	
}

/*----------------------------------------------------------------------------*/
bool Sim::addDebrisBody(int ID, Vec3f loc, float rad, float mass) {

	//only add gravBody if it's a thin sim
	if (isThin) {
		GravBody gbTemp;
		gbTemp.init(numDebris, loc, Vec3f(0.0f,0.0f,0.0f), rad, mass);
		debris[numDebris] = gbTemp;
		numDebris++;
		return true;
	}

	return false;
}

/*----------------------------------------------------------------------------*/
bool Sim::addOrbitalBody(int ID, Vec3f loc, float rad, float mass) {

	//only add gravBody if it's a thin sim
	if (isThin) {
		GravBody gbTemp;
		gbTemp.init(numBodies, loc, Vec3f(0.0f,0.0f,0.0f), rad, mass);
		bodies[numBodies] = gbTemp;		
		bodyOrder.push_back(gbTemp.getID());
		numBodies++;
		return true;
	}

	return false;
}

/*----------------------------------------------------------------------------*/
void Sim::userInit() {


	//bodyOrder = new int[numBodies];

	GravBody gbTemp;  // a temp gravBody to add to the map

	//init the planet
	gbTemp.init(0, Vec3f(0,0,0), Vec3f(0,0,0), 7, 1, (float)(rand()%30), ((rand()%5)+5)*.04);	
	bodies[0] = gbTemp;
	bodyOrder.push_back(gbTemp.getID());

	totalRad = 7;

	//add moons
	for ( int i = 1 ; i < numBodies ; i++ ) {

		float mass;
		float dist;
		float rad;
		float rotAxis, rotInc;


		float mRange = (rand()%100)*0.01f;
		float rRange = (rand()%100)*0.01f;
		mass = 0.001f+mRange*0.015f;
		dist = 20+i;
		rad = 0.05f+rRange*0.35f;
		
		rotAxis = (float)(rand()%30);
		rotInc = (rand()%2 ? 1 : -1)* ((rand()%10)+15)*.03;

		Vec3f loc = Vec3f(Vec3f::Random);
		loc *= dist;

		// Calculate +/- speed for circular orbit
		double thisG = G*1000; // Into milliseconds form
		float spd = (float)sqrt( G*(bodies[0].getMass()+mass) / dist );
		spd *= (rand()%2 ? 1 : -1);

		// Calculate angular velocity for circular orbit
		Vec3f moonToPlanet = *(bodies[0].getLoc()) - loc;
		Vec3f vel = moonToPlanet.crossedWith( Vec3f(0,0,1) );
		vel.normalizeToLength(spd);
	
		totalRad += rad;

		gbTemp.init(i,loc,vel,rad,mass,rotAxis,rotInc);
		bodies[i] = gbTemp;
		bodyOrder.push_back(gbTemp.getID());
	}


	//init debris bodies
	for ( int i = 0 ; i < numDebris ; i++ ) {

		float mass;
		float dist;
		float rad;

		float mRange = (rand()%100)*0.01f;
		float rRange = (rand()%100)*0.01f;
		mass = 0.001f+mRange*0.015f;
		dist = (rand()%300)+30.0f*i;
		rad = 0.05f+rRange*0.035f;


		Vec3f loc = Vec3f(Vec3f::Random);
		loc *= dist;
		Vec3f offsetLoc = Vec3f(Vec3f::Random);

		float spd = .05f;

		//set initial velocity
		Vec3f moonToPlanet = *(bodies[0].getLoc()) - loc;
		Vec3f vel = moonToPlanet + (offsetLoc * (float)(rand()%100));
		vel.normalizeToLength(spd);

		gbTemp.init(i, loc,vel,rad,mass);
		debris[i] = gbTemp;
	}
	

	//Asteroid Belt
	for (int j = numDebris; j < numDebris+100; j++)
	{
		float mass;
		float dist;
		float rad;

		float mRange = (rand()%100)*0.01f;
		float rRange = (rand()%100)*0.01f;
		mass = 0.001f+mRange*0.015f;
		dist = (rand()/65536.0f)*6 + 10.0f;
		rad = 0.05f+rRange*0.035f;


		Vec3f loc = Vec3f(Vec3f::Random);
		loc.z=0;
		loc.normalize();
		loc *= dist;
		loc.z += (rand()%5)*0.005f;
		float spd = .005f;

		//set initial velocity
		// Calculate angular velocity for circular orbit
		Vec3f moonToPlanet = *(bodies[0].getLoc()) - loc;
		Vec3f vel = moonToPlanet.crossedWith( Vec3f(0,0,0));
		vel.normalizeToLength(spd);

		gbTemp.init(j, loc,vel,rad,mass);
		debris[j] = gbTemp;
	}
	numDebris += 500;
}

/*----------------------------------------------------------------------------* /
void Sim::start() {
	while ( true ) { update(); }
}

/*----------------------------------------------------------------------------*/
void Sim::update() {

	timePrev = timeNow;
	timeNow = clock();

	// Only update if the clock has changed
	if ( !(timePass = timeNow-timePrev) ) { return; }

	if (!isThin)
		fatSimUpdate();
	else
		thinSimUpdate();
	
}

/*----------------------------------------------------------------------------*/
void Sim::fatSimUpdate() {

	float timeStep = timePass*timeDT;
	double currG = G*timeStep;

	bodies[0].updateRotation(timeStep);

	bodyMap::iterator SOBiter;
	missileMap::iterator mIter;

	bool wasCollision = false;
	
	// Start at one so planet is not affected by moons
	// NOTE: Collision handling requires that you increment the bodiesIter manually.
	for ( bodiesIter = bodies.begin() ; bodiesIter != bodies.end() ; ) {

		//if the id of the gravBody is zero, then this is the planet so skip it
		if (bodiesIter->first == 0) {
			++bodiesIter;		
			continue;
		}

		Vec3f sumForces = Vec3f(0,0,0);

		for ( SOBiter = bodies.begin() ; SOBiter != bodies.end() ; ++SOBiter ) {

			if ( SOBiter == bodiesIter ) { continue; }

			Vec3f AB = *(SOBiter->second.getLoc()) - *(bodiesIter->second.getLoc());
			float distSqr = AB.mag();

			if ( isCollision(&(bodiesIter->second), &(SOBiter->second), distSqr) ) {
				if ( SOBiter != bodies.begin() ) {
					handleMoonMoonCollision(&(bodiesIter->second), &(SOBiter->second));
					bodies.erase(SOBiter);
				}
				else {
					//SOBiter is the planet
					handleMoonPlanetCollision(&(bodiesIter->second), &(SOBiter->second));
				}
				bodies.erase(bodiesIter++);
				wasCollision = true;
				break; // Breaks out of SOBiter loop
			}

			AB.normalize();

			float massMult = bodiesIter->second.getMass()*SOBiter->second.getMass();
			float coeff = (float)(currG*massMult / distSqr);
			Vec3f force = AB * coeff;

			sumForces += force;
		}

		if ( wasCollision ) { wasCollision = false; continue; }

		//calc force from missiles	
		for ( mIter = missiles.begin() ; mIter != missiles.end() ; ++mIter ) {
			//skip if mass is 0
			if (mIter->second.getMMass() == 0)
				continue;

			Vec3f AB = *(mIter->second.getMLoc()) - *(bodiesIter->second.getLoc());
			float distSqr = AB.mag();

			AB.normalize();

			float massMult = bodiesIter->second.getMass()*mIter->second.getMMass();
			float coeff = (float)(currG*massMult / distSqr);
			Vec3f force = AB * coeff;
			if (force.length() > 0)
			   sumForces += force;
		}

		// Make inertia/mass a factor (because force is in Newtons)
		sumForces /= bodiesIter->second.getMass();

		bodiesIter->second.setForce(&sumForces);
		bodiesIter->second.updateWithForce(sumForces, timeStep);
		bodiesIter->second.updateHistory(timeStep);
		bodiesIter->second.updateFuture(sumForces, timeStep);
		bodiesIter->second.updateRotation(timeStep);

		++bodiesIter;

	}//end orbital body update


	//sort the orbital bodis array based on distance from the planet
	//sortBodies(0, numBodies);



	//calc force on debris
	for ( debrisIter = debris.begin() ; debrisIter != debris.end() ; ) {

		Vec3f sumForces = Vec3f(0, 0, 0);

		for ( SOBiter = bodies.begin() ; SOBiter != bodies.end() ; ++SOBiter ) {

			Vec3f AB = *(SOBiter->second.getLoc()) - *(debrisIter->second.getLoc());
			float distSqr = AB.mag();

			if ( isCollision(&(debrisIter->second), &(SOBiter->second), distSqr) ) {
				if ( SOBiter != bodies.begin() ) {
					handleDebrisMoonCollision(&(debrisIter->second), &(SOBiter->second));
				}
				else {
					//SOBiter is the planet
					handleDebrisPlanetCollision(&(debrisIter->second), &(SOBiter->second));
				}
				debris.erase(debrisIter++); // always erase the debris in a collision
				wasCollision = true;
				break; // Breaks out of SOBiter loop
			}

			AB.normalize();

			float massMult = debrisIter->second.getMass()*SOBiter->second.getMass();
			float coeff = (float)(currG*massMult / distSqr);
			Vec3f force = AB * coeff;

			sumForces += force;
		}

		if ( wasCollision ) { wasCollision = false; continue; }

		
		//calc force from missiles		
		for ( mIter = missiles.begin() ; mIter != missiles.end() ; ++mIter ) {
			//skip if mass is 0
			if (mIter->second.getMMass() == 0)
				continue;

			Vec3f AB = *(mIter->second.getMLoc()) - *(debrisIter->second.getLoc());
			float distSqr = AB.mag();

			AB.normalize();

			float massMult = debrisIter->second.getMass()*mIter->second.getMMass();
			float coeff = (float)(currG*massMult / distSqr);
			Vec3f force = AB * coeff;

			if (force.length() > 0)
			   sumForces += force;
		}


		//sumForces /= debris[a].getMass();		

		debrisIter->second.updateWithForce(sumForces, timeStep);
		//debrisIter->second.updateHistory(timeStep);


		//if its position has gone out of the star field then reset it
		//to some random new location on the outskirts of the field
		Vec3f* loc = (Vec3f*)debrisIter->second.getLoc();
		if ( loc->length() > 1000) {
			
			float dist;
			dist = (rand()%200)+30.0f*(rand()%50+1);

			Vec3f newLoc = Vec3f(Vec3f::Random);
			newLoc *= dist;
			Vec3f offsetLoc = Vec3f(Vec3f::Random);

			float spd = .05f;

			//set initial velocity
			Vec3f moonToPlanet = *(bodies[0].getLoc()) - newLoc;
			Vec3f vel = moonToPlanet + (offsetLoc * (float)(rand()%100));
			vel.normalizeToLength(spd);
			
			debrisIter->second.setLoc(&newLoc);
			debrisIter->second.setVel(&vel);
			//debrisIter->second.clearHistory();		
		}
		++debrisIter;
	}//end debris update


	//update the missiles
	for ( missilesIter = missiles.begin() ; missilesIter != missiles.end() ; ++missilesIter ) {
		missilesIter->second.updatePosition(timeStep);
		missilesIter->second.updateMissileHistory(timeStep);

		if (missilesIter->second.getProgress() >= .99 && !(missilesIter->second.isExploded()))
			missilesIter->second.explodeMissile();
		/*if (missilesIter->second.getOnTime() <= 0) {
			if ((int)missiles.size() == 1) {
				missiles.clear();
				break;
			}
			else
				//erasing takes too much work right now
				//It will set it's mass to 0;
				//missiles.erase(missilesIter++);
				
				
		}*/
	}	
}

/*----------------------------------------------------------------------------*/
void Sim::thinSimUpdate() {

	float timeStep = timePass*timeDT;
	//double currG = G*timeStep;

	//sort the orbital bodis array based on distance from the planet
	//sortBodies(0, numBodies);

	for ( bodiesIter = bodies.begin() ; bodiesIter != bodies.end() ; ++bodiesIter ) {

		//if the id of the gravBody is zero, then this is the planet so skip it
		if (bodiesIter->first == 0)
			continue;

		bodiesIter->second.updateHistory(timeStep);

		//Vec3f f = *bodies[a].getForce();
		//bodies[a].updateFuture(f, timeStep);
		//interpolate path, for smoother animation on client
		//Vec3f tv = *bodies[a].getVel() + f;
	    //Vec3f tl = *bodies[a].getLoc() + *bodies[a].getVel();
		//bodies[a].setLoc(&tl);
		//bodies[a].setVel(&tv);
	}

	for ( debrisIter = debris.begin() ; debrisIter != debris.end() ; ++debrisIter ) {
		//debrisIter->second.updateHistory(timeStep);

		Vec3f* loc = (Vec3f*)debrisIter->second.getLoc();
		if ( loc->length() > 950) {
			//debrisIter->second.clearHistory();
		}		
	}

}

/*----------------------------------------------------------------------------*/
void Sim::sortBodies(int beg, int end) {

	if (end > beg + 1)
	{
		float piv = bodies[bodyOrder[beg]].getLoc()->length();
		int l = beg + 1, r = end;
		while (l < r)
		{
			if (bodies[bodyOrder[l]].getLoc()->length() <= piv)
				l++;
			else
				swapBodies(l, --r);
		}
		swapBodies(--l, beg);
		sortBodies(beg, l);
		sortBodies(r, end);
	}

}

/*----------------------------------------------------------------------------*/
void Sim::swapBodies(int a, int b) {
	int temp = bodyOrder[a];
	bodyOrder[a] = bodyOrder[b];
	bodyOrder[b] = temp;
}

/*----------------------------------------------------------------------------*/
GravBody* Sim::getOrbitalBody(int index) {
	if (bodies.find(index) != bodies.end())
		return &bodies[index];
	else 
		return &bodies[0];
}

/*----------------------------------------------------------------------------*/
int Sim::getOrbitalBodyOrder(int index) {
	return bodyOrder[index];
}

/*----------------------------------------------------------------------------*/
GravBody* Sim::getDebrisBody(int index) {
	if (debris.find(index) != debris.end())
		return &debris[index];
	else 
		return &debris[0];
}

/*----------------------------------------------------------------------------*/
int Sim::getNumOrbitalBodies() { 
	return numBodies; 
}

/*----------------------------------------------------------------------------*/
int Sim::getNumDebrisBodies() { 
	return numDebris; 
}

/*----------------------------------------------------------------------------*/
float Sim::getTotalRad() {
	return totalRad;
}

/*----------------------------------------------------------------------------*/
void Sim::updateOrbitalBody(int id, Vec3f* loc, Vec3f* force) {

	//only allow "random" updates to 
	//gravBodies if we are the "thinSim"
	if (isThin) {

		bodies[id].setLoc(loc);
		bodies[id].setForce(force);
		Vec3f v = (*loc - *bodies[id].getOldLoc());
		bodies[id].setVel(&v);

		update();
	}
}

/*----------------------------------------------------------------------------*/
void Sim::updateDebrisBody(int id, Vec3f* loc) {

	//only allow "random" updates to 
	//gravBodies if we are the "thinSim"
	if (isThin) {
		debris[id].setLoc(loc);
	}
}

/*----------------------------------------------------------------------------*/
void Sim::thinSimStart() {
	if (isThin && !thinRdy) {
		thinRdy = true;
		//bodyOrder = new int[numBodies];
		//for (int i = 0; i < (int)bodies.size(); ++i)
		//	bodyOrder[i] = bodies[i].getID(); 
	}
}

/*----------------------------------------------------------------------------*/
bool Sim::isThinSimRdy() { return thinRdy; }

/*----------------------------------------------------------------------------*/
bodyMap* Sim::getOrbitalBodyMap() {
	return &bodies;
}

/*----------------------------------------------------------------------------*/
bodyMap* Sim::getDebrisBodyMap() {
	return &debris;
}


////////////////////////////////////////////////////////////////////////////////
// Collision functions
/*----------------------------------------------------------------------------*/
bool Sim::isCollision(GravBody* a, GravBody* b, float distSqr) {
	float radSum = a->getRad()+b->getRad();
	return (radSum*radSum >= distSqr);
}

/*----------------------------------------------------------------------------*/
void Sim::handleMoonMoonCollision(GravBody* m1, GravBody* m2) {
	// Both moons will be erased from the bodies map (in the physics loop)
	float mass1 = m1->getMass();
	float mass2 = m2->getMass();
	float m1MassRatio = mass1 / (mass1+mass2);
	Vec3f baseVel = *(m1->getVel())*m1MassRatio+*(m2->getVel())*(1-m1MassRatio);
	
	int genDebris = 80;
	int genDebris1 = (int)(genDebris*m1MassRatio);
	int genDebris2 = (int)(genDebris*(1-m1MassRatio));
	float debrisMass1 = mass1/(float)genDebris1;
	float debrisMass2 = mass2/(float)genDebris2;

	Vec3f loc1 = *(m1->getLoc());
	Vec3f loc2 = *(m2->getLoc());
	float radi1 = m1->getRad();
	float radi2 = m2->getRad();

	GravBody gbTemp;

	for ( int i = 0 ; i < genDebris1 ; i++ ) {
		Vec3f r = Vec3f(Vec3f::Random);
		Vec3f loc = loc1 + r*radi1;
		Vec3f vel = baseVel + r*(rand()/65536.0)*0.025;
		float rad = 0.03;
		gbTemp.init(numDebris, loc, vel, rad, debrisMass1);
		debris[numDebris++] = gbTemp;
	}
	
	for ( int i = 0 ; i < genDebris2 ; i++ ) {
		Vec3f r = Vec3f(Vec3f::Random);
		Vec3f loc = loc2 + r*radi2;
		Vec3f vel = baseVel + r*(rand()/65536.0)*0.025;
		float rad = 0.03;
		gbTemp.init(numDebris, loc, vel, rad, debrisMass2);
		debris[numDebris++] = gbTemp;
	}
}

/*----------------------------------------------------------------------------*/
void Sim::handleMoonPlanetCollision(GravBody* m, GravBody* p) {
	// The moon will be erased from the bodies map (in the physics loop)
	p->acceptMass(m);
}

/*----------------------------------------------------------------------------*/
void Sim::handleDebrisMoonCollision(GravBody* d, GravBody* m) {
	// The debris will be erased from the debris map (in the physics loop)
	m->acceptMass(d);
}

/*----------------------------------------------------------------------------*/
void Sim::handleDebrisPlanetCollision(GravBody* d, GravBody* p) {
	// The debris will be erased from the debris map (in the physics loop)
	p->acceptMass(d);
}


////////////////////////////////////////////////////////////////////////////////
// Missile functions
/*----------------------------------------------------------------------------*/
void Sim::createMissileBody(int id, Missile* mInfo, Vec3f loc, Vec3f dest) {
	MissileBody mbTemp;
	mbTemp.missileInit(id, mInfo, loc, dest);
	missiles[id] = mbTemp;
}

/*----------------------------------------------------------------------------*/
missileMap* Sim::getMissileBodyMap() {
	return &missiles;
}

/*----------------------------------------------------------------------------*/
void Sim::eraseMissileBody(int missileID) {
	missiles.erase(missiles.find(missileID));
}

